#ifndef SIMPLEMANEUVERINGFILTER_H
#define SIMPLEMANEUVERINGFILTER_H

//------------------------------------------------------------------------------
// Includes 
//------------------------------------------------------------------------------
#include <OpenCV/cv.h>
/**
 * SimpleManeuveringFilter is a class using a Kalman filter to implement a 
 * simple maneuvering filter. It provides a set of easy to use functions for 
 * parameter setup, and sets up the matrices for a reasonable simple filter. 
 * <p>
 * For each measurement, the Kalman filter keeps two state values: an
 * approximation of the "real" noise-less measurement, and
 * its rate of change (speed). All measurements are assumed uncorrolated, 
 * independent, but each have the same process noise, measurement noise, 
 * and maneuvering time. 
 * <p>
 * To use this class, construct an instance with the number of measurements
 * you want to use it for, set the various parameters (maneuvering time,
 * process noise stddev, measurement noise stddev), call init() with
 * an initial estimate of the state, and then call run() whenever you
 * have new measurements (or not). After every run(), the currently 
 * estimated state can be queried with getState().
 *
 * @author      Samuel Audet
 * @version     1.0, 2006-01-21
 * @since       1.0
 */
 
namespace Tracker
{
	class SimpleManeuveringFilter {
	public:
	    /** 
	     * SimpleManeuveringFilter constructor specifiying the number of measurements
	     * we would like to filter with this filter. 
	     */
	    SimpleManeuveringFilter(int measurementDimension = 1);
	    virtual ~SimpleManeuveringFilter();
	    
	    
	    /**
	     * Sets the maneuvering time of the filter. The maneuvering time 
	     * consists of a constant indicating how many calls to run() it takes 
	     * in average for a state to stop changing after starting to change. 
	     * The change is assumed to take place at a constant rate (constant 
	     * speed, no acceleration).
	     *
	     * @param tau       the maneuvering time in number of calls to run()
	     */
	    void setManeuveringTime(double tau = 30) { this->tau = tau; }
	    /**
	     * Sets the standard deviation of the process noise.
	     * Indicates the change in the rate of change (acceleration) that usually
	     * happens in the process, but that is not modeled by this filter.
	     *
	     * @param s         the standard deviation of the process noise
	     */
	    void setProcessStdDev(double s = 1)      { sProcess = s; }
	    /**
	     * Sets the standard deviation of the measurement noise.
	     *
	     * @param s         the standard deviation of the measurement noise
	     */
	    void setMeasurementStdDev(double s = 1)  { sMeasurement = s; }
	    /**
	     * Returns the maneuvering time in number of calls to run().
	     *
	     * @return          the maneuvering time in number of calls to run()
	     */
	    double getManeuveringTime() const   { return tau; }
	    /**
	     * Returns the standard deviation of the process noise.
	     *
	     * @return          the standard deviation of the process noise.
	     */
	    double getProcessStdDev() const     { return sProcess; }
	    /**
	     * Returns the standard deviation of the measurement noise.
	     *
	     * @return          the standard deviation of the measurement noise.
	     */
	    double getMeasurementStdDev() const { return sMeasurement; }
	
	
	    /**
	     * Initializes this filter with the parameters (process and measurement 
	     * noises, maneuvering time) and the initial state state0. 
	     *
	     * @param state0    [0] = initial "real" noise-less value of the first measurement, 
	     *                  [1] = initial guess at the rate of change of the first measurement
	     *                  [2] = initial "real" noise-less value of the second measurement, etc.
	     */
	    void init(const double state0[]);
	    /**
	     * Runs the filter with the given measurement. If measurement == NULL,
	     * runs the filter without measurement (no correction).
	     *
	     * @param measurement    [0] = first noisy measurement
	     *                       [1] = second noisy measurement, etc.
	     */
	    void run(const double measurement[]);
	    /**
	     * Gets the current state as modeled by this filter. 
	     *
	     * @param state     [0] = approximation of the "real" noise-less value of the first measurement, 
	     *                  [1] = guess at the rate of change of the first measurement
	     *                  [2] = approximation of the "real" noise-less value of the second measurement, etc.
	     */
	    void getState(double state[]) const;
	    /**
	     * Sets the state without initializing this filter with new parameters.
	     *
	     * @param state     [0] = "real" noise-less value of the first measurement, 
	     *                  [1] = guess at the rate of change of the first measurement
	     *                  [2] = "real" noise-less value of the second measurement, etc.
	     */
	    void setState(const double state[]);
	
	private:
	    CvKalman *kalman;
	    double tau, sProcess, sMeasurement;
	
	    int measurementDimension; // number of data points 
	    int stateDimension;       // stateDimension = 2 * measurementDimension, 
	                              // keeps approximations of "real" measurement + rate of change
	};
}

#endif // SIMPLEMANEUVERINGFILTER_H
